#include "AngleSolver.h"

AngleSolver::AngleSolver()
{
}

AngleSolver::~AngleSolver()
{
}

void AngleSolver::setCameraParam(const cv::Mat & camera_matrix, const cv::Mat & distortion_coeff)
{
	camera_matrix.copyTo(CAMERA_MATRIX);
	distortion_coeff.copyTo(DISTORTION_COEFF);
}

int AngleSolver::setCameraParam(const char * filePath, int camId)
{
	FileStorage fsRead;
	fsRead.open(filePath, FileStorage::READ);
	if (!fsRead.isOpened())
	{
		cout << "Failed to open xml" << endl;
		return -1;
	}

	fsRead["Y_DISTANCE_BETWEEN_GUN_AND_CAM"] >> GUN_CAM_DISTANCE_Y;

	Mat camera_matrix;
	Mat distortion_coeff;
	switch (camId)
	{
	case 1:
		fsRead["CAMERA_MATRIX_1"] >> camera_matrix;
		fsRead["DISTORTION_COEFF_1"] >> distortion_coeff;
		break;
	case 2:
		fsRead["CAMERA_MATRIX_2"] >> camera_matrix;
		fsRead["DISTORTION_COEFF_2"] >> distortion_coeff;
		break;
	case 3:
		fsRead["CAMERA_MATRIX_3"] >> camera_matrix;
		fsRead["DISTORTION_COEFF_3"] >> distortion_coeff;
		break;
	default:
		cout << "WRONG CAMID GIVEN!" << endl;
		break;
	}
	setCameraParam(camera_matrix, distortion_coeff);
	fsRead.release();
	return 0;
}

void AngleSolver::setBulletSpeed(int bulletSpeed)
{
	BULLET_SPEED = bulletSpeed;
}

void AngleSolver::setTarget(Point2f centerPoint)
{
	targetCenter = centerPoint;
}

void AngleSolver::setDistance(double distance)
{
	this->distance = distance;
}

void AngleSolver::solveAngles()
{
	// ONLY PinHole Solver
	double fx = CAMERA_MATRIX.at<double>(0, 0);
	double fy = CAMERA_MATRIX.at<double>(1, 1);
	double cx = CAMERA_MATRIX.at<double>(0, 2);
	double cy = CAMERA_MATRIX.at<double>(1, 2);
	Point2f pnt;
	vector<cv::Point2f> in;
	vector<cv::Point2f> out;
	in.push_back(targetCenter);

	//对像素点去畸变
	undistortPoints(in, out, CAMERA_MATRIX, DISTORTION_COEFF, noArray(), CAMERA_MATRIX);
	pnt = out.front();

	//去畸变后的比值
	double rxNew = (pnt.x - cx) / fx;
	double ryNew = (pnt.y - cy) / fy;

	y_yaw = atan(rxNew) / CV_PI * 180;
	x_pitch = -atan(ryNew) / CV_PI * 180;

}

void AngleSolver::compensateAngle()
{
	// compensate Offset(Camera-Gun Offset)
	float camera_target_height = distance * sin(x_pitch / 180 * CV_PI);
	float gun_target_height = camera_target_height + GUN_CAM_DISTANCE_Y;
	float gun_pitch_tan = gun_target_height / (distance * cos(x_pitch / 180 * CV_PI));
	x_pitch = atan(gun_pitch_tan) / CV_PI * 180;

	// compensate Gravity
	float compensateGravity_pitch_tan = tan(x_pitch / 180 * CV_PI) + (0.5*9.8*(distance / BULLET_SPEED)*(distance / BULLET_SPEED)) / cos(x_pitch / 180 * CV_PI);
	x_pitch = atan(compensateGravity_pitch_tan) / CV_PI * 180;
}

void AngleSolver::getAngle(Point2f cenerPoint, double & yaw, double & pitch)
{
	setTarget(cenerPoint);
	solveAngles();
	//compensateAngle();
	yaw = y_yaw;
	pitch = x_pitch;
}

void AngleSolver::showDebugInfo(bool showCurrentResult, bool showPinHole, bool showCompensation, bool showCameraParams)
{
	if (showCurrentResult)
	{
		cout << "Yaw: " << y_yaw << "Pitch: " << x_pitch << "Distance: " << distance << endl;
		cout << "-----------------------------------------------" << endl;
	}

	float yaw_temp = y_yaw, pitch_temp = x_pitch;
	if (showPinHole)
	{
		solveAngles();
		cout << "PinHole Solver:" << endl;
		cout << "Yaw: " << y_yaw << "Pitch: " << x_pitch << endl;
		cout << "-----------------------------------------------" << endl;
		y_yaw = yaw_temp; x_pitch = pitch_temp;
	}
	if (showCompensation)
	{
		solveAngles();
		float raw_pitch;
		raw_pitch = x_pitch;

		solveAngles();
		compensateAngle();
		cout << "Compensation:" << endl;
		cout << "Pitch Compensation:" << x_pitch - raw_pitch << endl;
		cout << "Yaw: " << y_yaw << "Pitch: " << x_pitch << endl;
		cout << "-----------------------------------------------" << endl;
		y_yaw = yaw_temp; x_pitch = pitch_temp;
	}
	if (showCameraParams)
	{
		cout << "CANERA_MATRIX:" << endl;
		cout << CAMERA_MATRIX << endl;
		cout << "-----------------------------------------------" << endl;
		cout << "DISTORTION_COEFF" << endl;
		cout << DISTORTION_COEFF << endl;
		cout << "-----------------------------------------------" << endl;
	}
}